package hk.edu.polyu.comp.motioncapture.collection;

import hk.edu.polyu.comp.motioncapture.point.*;

import java.util.Iterator;

public class DimensionGeometry {

	public static final double radCircle = Math.PI * 2;
	public static final double radHalfCircle = Math.PI;

	/**
	 * Square the <code>value</code>.
	 * 
	 * @param value
	 * @return squared value
	 */
	public static double square(double value) {
		return value * value;
	}

	/**
	 * Check if the two points is within the distance range.
	 * 
	 * @param pt1
	 *            Point A
	 * @param pt2
	 *            Point B
	 * @param distance
	 *            to check with
	 * @return true if two points is in range
	 */
	public static boolean withinDistance(Point pt1, Point pt2, double distance) {
		return square(pt1.getX() - pt2.getX())
				+ square(pt1.getY() - pt2.getY()) <= square(distance);
	}

	/**
	 * Get the distance between two points.
	 * 
	 * @param pt1
	 *            Point A
	 * @param pt2
	 *            Point B
	 * @return distance between two point
	 */
	public static double distance(Point pt1, Point pt2) {
		return Math.sqrt((pt1.getX() - pt2.getX()) * (pt1.getX() - pt2.getX())
				+ (pt1.getY() - pt2.getY()) * (pt1.getY() - pt2.getY()));
	}

	public static Point midPoint(Point pt1, Point pt2) {
		double x = (pt1.getX() + pt2.getX()) / 2;
		double y = (pt1.getY() + pt2.getY()) / 2;

		return new Point(x, y);
	}

	/**
	 * Get the area with four points
	 * 
	 * @param pt
	 *            A optiPoint array
	 * @return Area in double
	 */
	public static double fourPointArea(Frame pt) {
		try {
			double area = 0;
			Frame temp = new Frame(3, 1);

			// 012
			temp.add(pt.pointAt(0));
			temp.add(pt.pointAt(1));
			temp.add(pt.pointAt(2));
			double area1 = threePointArea(temp);

			// 023
			temp.add(pt.pointAt(0));
			temp.add(pt.pointAt(2));
			temp.add(pt.pointAt(3));
			double area2 = threePointArea(temp);

			// 013
			temp.add(pt.pointAt(0));
			temp.add(pt.pointAt(1));
			temp.add(pt.pointAt(3));
			double area3 = threePointArea(temp);

			// 123
			temp.add(pt.pointAt(1));
			temp.add(pt.pointAt(2));
			temp.add(pt.pointAt(3));
			double area4 = threePointArea(temp);

			area = (area1 + area2 + area3 + area4) / 2;

			return area;
		} catch (Exception exc) {
			return -1;
		}

	}

	/**
	 * Get the area formed by three points
	 * 
	 * @param opt
	 *            A Point vector array with 3 elements
	 * @return Area in double
	 */
	public static double threePointArea(Frame pt) {
		double area = 0;

		if (pt.size() == 3) {
			Point pt1 = pt.pointAt(0);
			Point pt2 = pt.pointAt(1);
			Point pt3 = pt.pointAt(2);

			area = Math.abs(pt1.getX() * pt2.getY() + pt2.getX() * pt3.getY()
					+ pt3.getX() * pt1.getY() - pt1.getX() * pt3.getY()
					- pt3.getX() * pt2.getY() - pt2.getX() * pt1.getY()) / 2;
		}
		return area;
	}

	/**
	 * Get the intersection point of the two diagonal lines formed by the four
	 * points
	 * 
	 * @param opt1
	 *            Point 1
	 * @param opt2
	 *            Point 2
	 * @param opt3
	 *            Point 3
	 * @param opt4
	 *            Point 4
	 * @return Intersection Point in Point
	 */
	public static Point intersectionPoint(Point pt1, Point pt2, Point pt3,
			Point pt4) {
		double pux = ((pt3.getX() - pt1.getX()) * (pt2.getY() - pt1.getY()) - (pt3
				.getY() - pt1.getY())
				* (pt2.getX() - pt1.getX()))
				/ ((pt3.getY() - pt1.getY()) * (pt4.getX() - pt2.getX()) - (pt3
						.getX() - pt1.getX())
						* (pt4.getY() - pt2.getY()));

		double pmx = pt2.getX() + pux * (pt4.getX() - pt2.getX());

		double puy = ((pt4.getX() - pt2.getX()) * (pt2.getY() - pt1.getY()) - (pt4
				.getY() - pt2.getY())
				* (pt2.getX() - pt1.getX()))
				/ ((pt3.getY() - pt1.getY()) * (pt4.getX() - pt2.getX()) - (pt3
						.getX() - pt1.getX())
						* (pt4.getY() - pt2.getY()));

		double pmy = pt2.getY() + puy * (pt4.getY() - pt2.getY());

		return new Point(pmx, pmy);
	}

	/**
	 * Get the slope of the line between two points
	 * 
	 * @param opt1
	 *            Point 1
	 * @param opt2
	 *            Point 2
	 * @return Slope in double
	 */
	public static double slope(Point opt1, Point opt2) {
		if (opt2.getX() - opt1.getX() == 0) {
			return Double.MAX_VALUE;
		} else {
			return Math.toDegrees(Math.atan((opt2.getY() - opt1.getY())
					/ (opt2.getX() - opt1.getX())));
		}
	}

	/**
	 * Get the points bounding the whole point set, order by clockwise
	 * 
	 * @param pts
	 *            Point Set
	 * @return Point set in Point Vector
	 */
	public static Frame giftWrappingBound(Frame pts) {
		Frame ptBound = new Frame();
		Frame ptTemp = new Frame();
		Iterator<Point> itPt = null;
		Point pt = null, initPt = null;
		Point leftPt = null, rightPt = null;
		Point lastPt = null;
		double rightAngle = 0, currentAngle = 0;
		int ptCount = 0;

		// Gift Wrapping Algorithm to find the bounding rectangle
		// 1. Find leftmost point
		double minX = Double.MAX_VALUE;

		itPt = pts.iterator();
		while (itPt.hasNext()) {
			pt = itPt.next();
			ptTemp.add(pt); // Move all points to the temp list
			if (pt.getX() < minX) {
				minX = pt.getX();
				leftPt = pt;
			}
		}
		if (leftPt == null)
			return new Frame();

		initPt = leftPt;
		lastPt = initPt.add(new Point(0, -1));

		// 2. Find a point such that all other points are to the right of the
		// line
		boolean done = false;
		// Go thru every possible points
		while (ptTemp.size() > 0 && !done) {
			rightPt = null;
			itPt = ptTemp.iterator();
			while (itPt.hasNext()) {
				pt = itPt.next();
				if (pt != leftPt) {
					if (rightPt == null) {
						rightPt = pt;
						rightAngle = polarAngle(lastPt, leftPt, pt);
					} else {
						currentAngle = polarAngle(lastPt, leftPt, pt);
						if (currentAngle < rightAngle) {
							rightPt = pt;
							rightAngle = currentAngle;
						}
					}
				}
			}
			// Add the point to list and move to other point
			if (rightPt != null) {
				ptBound.add(rightPt);
				ptTemp.remove(rightPt);
				ptCount++;
				if (rightPt == initPt) {
					done = true;
				} else {
					lastPt = leftPt;
					leftPt = rightPt;
				}
			} else
				done = true;
		}

		return ptBound;
	}

	/**
	 * Get the polar radius from optOrg to optTgt, when the x-axis has the angle
	 * of zero degree
	 * 
	 * @param origin
	 *            Origin Point
	 * @param target
	 *            Target Point
	 * @return
	 */
	public static double polarRadius(Point origin, Point target) {
		double x = target.getX() - origin.getX();
		double y = target.getY() - origin.getY();
		return Math.sqrt(x * x + y * y);
	}

	/**
	 * Get the polar angle from optOrg to optTgt, when the x-axis has the angle
	 * of zero degree
	 * 
	 * @param origin
	 *            Origin Point
	 * @param target
	 *            Target Point
	 * @return Polar angle in double, range from 0 to 360
	 */
	public static double polarAngle(Point origin, Point target) {
		double x = target.getX() - origin.getX();
		double y = target.getY() - origin.getY();
		double rad = Math.atan2(y, x);
		if (rad < 0) {
			rad = radCircle + rad;
		}
		return rad;
		// return (rad + (Math.PI / 2)) % (Math.PI * 2);
		// return (Math.PI * 2) - ((rad + (Math.PI / 2 * 3)) % (Math.PI * 2));
		// double angrad = Math.atan2(target.getY() - origin.getY(),
		// target.getX()
		// - origin.getX());
		// double degree = 0;
		// if (angrad < 0)
		// degree = Math.toDegrees(angrad + Math.PI * 2);
		// else
		// degree = Math.toDegrees(angrad);
		// degree = ((360 - degree) + 90) % 360;
		//
		// return degree;
	}

	public static double polarAngle(Point origin, Point midPoint, Point target) {
		double rad1 = polarAngle(origin, midPoint);
		double rad2 = polarAngle(midPoint, target);

		if (rad1 < radHalfCircle && rad2 > radHalfCircle) {
			return radCircle - rad2 + radHalfCircle + rad1;
		} else {
			return Math.abs(rad1 - rad2);
		}
	}
}
